




m = 6.25;              %参与俯仰的重量
h = 0.2156;            %0.13是中间点  0.2156是最高点  0.085是最低点 单位m 
I = m*h^2;           %转动惯量
g = 9.8;             %重力加速度
r = 0.07;            %轮子半径  单位m
M = 3.7;             %不参与俯仰的重量 单位KG
D = (M+m)*(m*h^2 + I) - m^2*h^2;


A = [0 1 0 0; ((M+m)*m*g*h)/D 0 0 0;0 0 0 1;-(m^2*g*h^2)/D 0 0 0];
B = [0; -(m*h)/(D*r);0;1/((M+m)*r)-(m^2*h^2)/((M+m)*D*r)];
Q = diag([1 1 1 1]);
R = 1;

K = lqr(A, B, Q, R)
